{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Determine derivative of Jacobian from angular velocity to exponential rates\n",
    "\n",
    "Peter Corke 2021, updated 1/23\n",
    "\n",
    "SymPy code to determine the time derivative of the mapping from angular velocity to exponential coordinate rates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A rotation matrix can be expressed in terms of exponential coordinates (also called the Euler vector)\n",
    "\n",
    "$\n",
    "\\mathbf{R} = e^{[\\varphi]_\\times} \n",
    "$\n",
    "where $\\mathbf{R} \\in SO(3)$ and $\\varphi \\in \\mathbb{R}^3$.\n",
    "\n",
    "The mapping from angular velocity $\\omega$ to exponential coordinate rates $\\dot{\\varphi}$ is\n",
    "\n",
    "$\n",
    "\\dot{\\varphi} = \\mathbf{A} \\omega\n",
    "$\n",
    "\n",
    "where $\\mathbf{A}$ is given by (2.107) of [Robot Dynamics Lecture Notes, Robotic Systems Lab, ETH Zurich, 2017](https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf)\n",
    "\n",
    "\n",
    "$\n",
    "\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [\\varphi]_\\times + [\\varphi]^2_\\times \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right),\n",
    "$\n",
    "where $\\theta = \\| \\varphi \\|$\n",
    "\n",
    "We simplify the equation as\n",
    "\n",
    "$\n",
    "\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [\\varphi]_\\times + [\\varphi]^2_\\times \\Theta\n",
    "$\n",
    "\n",
    "where\n",
    "$\n",
    "\\Theta = \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right)\n",
    "$\n",
    "\n",
    "We can find the derivative using the chain rule\n",
    "\n",
    "$\n",
    "\\dot{\\mathbf{A}} = - \\frac{1}{2} [\\dot{\\varphi}]_\\times +  \\left( [\\varphi]_\\times [\\dot{\\varphi}]_\\times + [\\dot{\\varphi}]_\\times[\\varphi]_\\times  \\right) \\Theta + [\\varphi]^2_\\times \\dot{\\Theta}\n",
    "$\n",
    "\n",
    "noting that the derivative of a matrix squared is\n",
    "\n",
    "$\n",
    "\\frac{d}{dt} (\\mathbf{M}^2) = (\\frac{d}{dt} \\mathbf{M}) \\mathbf{M} + \\mathbf{M} (\\frac{d}{dt} \\mathbf{M})\n",
    "$\n",
    "\n",
    "We start by defining some symbols"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta, theta_dot, t = symbols('theta theta_dot t', real=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We start by finding an expression for $\\Theta$ which depends on $\\theta(t)$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta_t = Function(theta)(t)\n",
    "theta_t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Theta = 1 / theta_t ** 2 * (1 - theta_t / 2 * sin(theta_t) / (1 - cos(theta_t)))\n",
    "Theta"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and now determine the derivative"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T_dot = Theta.diff(t)\n",
    "T_dot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which is a somewhat complex expression that depends on $\\theta(t)$ and $\\dot{\\theta}(t)$.\n",
    "\n",
    "We will remove the time dependency and generate code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T_dot = T_dot.subs([(theta_t.diff(t), theta_dot), (theta_t, theta)])\n",
    "T_dot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pycode(T_dot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to evaluate the line above we need an expression for $\\theta$ and $\\dot{\\theta}$.  $\\theta$ is the norm of $\\varphi$ whose elements are functions of time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "phi_names = ('varphi_0', 'varphi_1', 'varphi_2')\n",
    "phi = []  # names of angles, eg. theta\n",
    "phi_t = []  # angles as function of time, eg. theta(t)\n",
    "phi_d = []  # derivative of above, eg. d theta(t) / dt\n",
    "phi_n = []  # symbol to represent above, eg. theta_dot\n",
    "for i in phi_names:\n",
    "    phi.append(symbols(i, real=True))\n",
    "    phi_t.append(Function(phi[-1])(t))\n",
    "    phi_d.append(phi_t[-1].diff(t))\n",
    "    phi_n.append(i + '_dot')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compute the norm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta = Matrix(phi_t).norm()\n",
    "theta"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and find its derivative"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta_dot = theta.diff(t)\n",
    "theta_dot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and now remove the time dependenices"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "theta_dot = theta_dot.subs(a for a in zip(phi_d, phi_n))\n",
    "theta_dot = theta_dot.subs(a for a in zip(phi_t, phi))\n",
    "theta_dot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which is simply the dot product over the norm."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "A, t = symbols('A t', real=True)\n",
    "A_t = Function(A)(t)\n",
    "d = diff(exp(A_t), t)\n",
    "print(d)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.5 ('dev')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.15"
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
